#ifndef _POSERESOLVE_H
#define _POSERESOLVE_H
#include <stdio.h>
#include <stack>
#include <opencv2/opencv.hpp>
using namespace cv;

using namespace std;
//using Eigen3
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <complex>
#include <math.h>
#include <cfloat>
#include<limits.h>  //int max

//DJI_CAM HEAD
#include <stdio.h>
#include <setjmp.h>
#include <stdlib.h>
#include <malloc.h>
#include <string.h>
#include <unistd.h>
#include <signal.h>
#include <assert.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <getopt.h>
#include <pthread.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <ctype.h>

//
//apriltag
//*************

#include <cstring>
#include <vector>
#include <list>
#include <sys/time.h>
const string usage = "\n"
  "Usage:\n"
  "  apriltags_demo [OPTION...] [IMG1 [IMG2...]]\n"
  "\n"
  "Options:\n"
  "  -h  -?          Show help options\n"
  "  -a              Arduino (send tag ids over serial port)\n"
  "  -d              Disable graphics\n"
  "  -t              Timing of tag extraction\n"
  "  -C <bbxhh>      Tag family (default 36h11)\n"
  "  -D <id>         Video device ID (if multiple cameras present)\n"
  "  -F <fx>         Focal length in pixels\n"
  "  -W <width>      Image width (default 640, availability depends on camera)\n"
  "  -H <height>     Image height (default 480, availability depends on camera)\n"
  "  -S <size>       Tag size (square black frame) in meters\n"
  "  -E <exposure>   Manually set camera exposure (default auto; range 0-10000)\n"
  "  -G <gain>       Manually set camera gain (default auto; range 0-255)\n"
  "  -B <brightness> Manually set the camera brightness (default 128; range 0-255)\n"
  "\n";

const string intro = "\n"
    "April tags test code\n"
    "(C) 2012-2014 Massachusetts Institute of Technology\n"
    "Michael Kaess\n"
    "\n";

#ifndef __APPLE__
#define EXPOSURE_CONTROL // only works in Linux
#endif

#ifdef EXPOSURE_CONTROL
#include <libv4l2.h>
#include <linux/videodev2.h>
#include <fcntl.h>
#include <errno.h>
#endif

// April tags detector and various families that can be selected by command line option
#include "AprilTags/TagDetector.h"
#include "AprilTags/Tag16h5.h"
#include "AprilTags/Tag25h7.h"
#include "AprilTags/Tag25h9.h"
#include "AprilTags/Tag36h9.h"
#include "AprilTags/Tag36h11.h"

// Needed for getopt / command line options processing
#include <unistd.h>
extern int optind;
extern char *optarg;

#include <cmath>
#ifndef PI
const double PI = 3.14159265358979323846;
#endif
const double TWOPI = 2.0*PI;
//AprilTags

#define SHOW_IMAGE               0  //define if show image or draw image
#define SAVE_IMAGE               0
#define FRAME_SIZE              (1280*720*3/2)  /*format NV12*/
#define BLOCK_MODE                     1
#define DISPLAY_MODE            (1 << 0)
#define GETBUFFER_MODE          (1 << 1)
#define TRANSFER_MODE           (1 << 2)
#define CAM_BLOCK                1
#define CAM_NON_BLOCK            0
#define PI 3.141593
struct EllipsePara
{
    Point2f c;
    float A;
    float B;
    float C;
    float D;
    float E;
    float F;
};

struct CamPos
{
    double Position_X;
    double Position_Y;
    double Position_Z;
    double Distance;
    double roll;
    double pitch;
    double yaw;

    Point2f PosMcircle;  //position of tracked point
    bool tar_detec_flag; //0:not detect
    bool gimbal_mode;    //0:search mode,1:tract mode
};

struct TargetRect
{
    Rect CirclesRect;
    Rect AprilTagsRect;
    Rect K_CirclesRect;
    Rect K_AprilTagsRect;
    int TargetMode;     //0:circles,1:apriltags
};

void push_feature_points_position();
void far_resolve(Mat frame);
Rect far_target_detecion(Mat src, Rect ROI_rec, Mat dst );
Rect far_target_fitellipse(Mat src,Rect rec, struct EllipsePara *Ep_t, int Ep_num);
Rect rectCenterScale(Rect rect,float scale);
void far_cal_tangent_para(Mat src, struct EllipsePara *Ep_t,
                          Eigen::Matrix<std::complex< double >, 4, 1> &x,
                          Eigen::Matrix<std::complex< double >, 4, 1> &y);
void far_cal_feature_points(Mat src, struct EllipsePara *Ep_t,
                            Eigen::Matrix<std::complex< double >, 4, 1> &para_x,
                            Eigen::Matrix<std::complex< double >, 4, 1> &para_y,
                            Point2f *feature_points);
void far_cal_camera_position(Mat src, Point2f *feature_points, Mat rvecsMat, Mat tvecsMat, Mat Cam_position);
Point2f get_cross_point_2f(Point2f pt1,Point2f pt2,Point2f pt3,Point2f pt4);

void quadratic2quartic(Eigen::Matrix<double, 6, 1> & _par1,
                       Eigen::Matrix<double, 6, 1> & _par2,
                       Eigen::Matrix<double, 5, 1> & _y2x_par,
                       Eigen::Matrix<std::complex<double>, 5, 1> &_quartic_par);

void sqrtn(const std::complex<double> &_input,
           double n,
           std::complex<double> & _out);

void Ferrari(Eigen::Matrix<std::complex<double>, 5, 1> & _quartic_par,
             Eigen::Matrix<std::complex<double>, 4, 1> & _x);

void compute_y(Eigen::Matrix<std::complex<double>, 4, 1> &_x,
           Eigen::Matrix<double, 5, 1> &_y2x_par,
           Eigen::Matrix<double, 6, 1> & _ic1,
           Eigen::Matrix<double, 6, 1> & _ic2,
           Eigen::Matrix<std::complex<double>, 4, 1> &_y);

void near_resolve(Mat frame);

#endif
